import subprocess
import time
import os

def main():
    # 切换到 catkin_ws 目录（按需取消注释）
    # os.chdir(os.path.expanduser("~/catkin_ws"))
    
    # 加载 ROS 环境变量（改进方式）
    subprocess.run("source /opt/ros/noetic/setup.bash && source ~/catkin_ws/devel/setup.bash", 
                   shell=True, executable='/bin/bash')
    
    # 设置串口权限（按需取消注释）
    # subprocess.run("sudo chmod 777 /dev/ttyUSB0", shell=True)
    
    # 启动 MAVROS（后台运行）
    mavros_process = subprocess.Popen(
        "roslaunch mavros apm.launch",
        shell=True
    )
    time.sleep(5)
    
    # 设置 MAVROS 速率
    rate_process = subprocess.Popen(
        "rosrun mavros mavsys rate --all 10",
        shell=True
    )
    print("rate_change")
    
    # 启动 AR 视觉跟踪
    ar_tracking_process = subprocess.Popen(
        "roslaunch yahboomcar_visual ar_track_csi.launch",
        shell=True
    )
    time.sleep(4)
    
    # 启动 GPS + 视觉降落
    gps_landing_process = subprocess.Popen(
        "rosrun iq_gnc 124GPSvisionland",
        shell=True
    )
    
    print("所有ROS节点已成功启动")

if __name__ == "__main__":
    main()